#include "SR04.h"

SR04::SR04(int echoPin, int triggerPin) {
    _echoPin = echoPin;
    _triggerPin = triggerPin;
    _duration=0;
    start_time=0;
    end_time=0;
	wiringPiSetup();
    pinMode(_echoPin, INPUT);
    pinMode(_triggerPin, OUTPUT);
}

long SR04::Distance() {
    long d = 0;
	int IS_first = 1;//是否为第一次
    long tmp,tmp1;
    struct timeval start_tv, end_tv, tmp_tv;
    _duration = 0;
	
    digitalWrite(_triggerPin, LOW);
    delayMicroseconds(2);
    digitalWrite(_triggerPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(_triggerPin, LOW);
    delayMicroseconds(2);
    
	//记录时间
	while(digitalRead(_echoPin)==0){//开始时间
		if(IS_first == 1){
			gettimeofday(&tmp_tv, NULL);
			IS_first = 0;
		}
		//printf("echo \n");
		gettimeofday(&start_tv, NULL);
		if(start_tv.tv_sec - tmp_tv.tv_sec > 1){//超时直接终止
			return -1;
		}
	}
	IS_first = 1;
	while(digitalRead(_echoPin)==1){//结束时间
		if(IS_first == 1){//记录第一次时间，超时直接终止
			gettimeofday(&tmp_tv, NULL);
			IS_first = 0;
		}
		//printf("echo LOW\n");
		gettimeofday(&end_tv, NULL);
		if(end_tv.tv_sec - tmp_tv.tv_sec > 1){//超时直接终止
			return -1;
		}
	}
	tmp=end_tv.tv_sec-start_tv.tv_sec;
	tmp1=end_tv.tv_usec-start_tv.tv_usec;
	//printf("end_sec:%ld  start_sec:%ld\n",end_tv.tv_sec,start_tv.tv_sec);
	//printf("end_usec:%ld  start_usec:%ld\n",end_tv.tv_usec,start_tv.tv_usec);
	//printf("tmp:%ld  tmp1:%ld\n",tmp,tmp1);
	_duration=tmp*1000*1000+tmp1;
	//printf("duration:%ld\n",_duration);

	d=(_duration*100)/5831;
	//printf("d:%ld\n",d);
    return d;
}

long SR04::Filter_Distance(){
	int count;
    long value_buf[SR04_NUM], sum = 0;
    for( count = 0; count < SR04_NUM; count++ ){
        value_buf[count] =  Distance();
		if(value_buf[count] == -1){
			return -1;
		}
		delay(5);
    }
	sort(value_buf, value_buf + SR04_NUM);//sort
    for( count = 1; count < SR04_NUM - 1; count++ ){
        sum += value_buf[count];
    }
    return (long)( sum / ( SR04_NUM - 2 ) );
}
